/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file dataToXML.cxx
	\brief class implemenation XML generator

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <stdio.h>
#include <math.h>

#include "mavSim.h"
#include "utilities.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
/*! \brief determines level of output to user. */
extern bool verbose;
/*<-End Global Variables-->*/

/************************************************************\
|                  DataToXML_Parser::init                    |
\************************************************************/
/*!
  \brief the initializing function for the XML generating thread.
  \param shm shared memory struct between the gateway threads.
  \pre instance of DataToXML_Parser has been created
  \post the XML utils have been intialized and the inetTxFifo
  and SAVRxFifo have been opened
  \return TRUE on successful completion of initialization, FALSE otherwise
*/

MAVSimThread::MAVSimThread(String name) : ThreadedPipe(name)
{
	waypoints[0].number = 0;
	waypoints[0].next = 0;
	waypoints[0].orbitRad = 100;
	waypoints[0].pos.lat = 0.0;
	waypoints[0].pos.lon = 0.0;
	waypoints[0].pos.alt = 0;

	gps.utc.hours=0;
	gps.utc.minutes=0;
	gps.utc.seconds=0.0;
	current_waypoint = 0;
	gps.pos.lat = waypoints[current_waypoint].pos.lat;
	gps.pos.lon = waypoints[current_waypoint].pos.lon;
	gps.track=0;
	gps.pos.alt=0;
	gps.sog=0;
	gps.accuracy.quality=0;
	gps.accuracy.sats=0;
	gps.accuracy.pdop=0.0;

	current_waypoint = waypoints[current_waypoint].next;
	velocity.x = 0;
	velocity.y = -10;

	parent = NULL;
	partner = NULL;

	phase.angle = 0.0;
	speed = 15.0;
}

bool MAVSimThread::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	last_telem_tx.stamp();
	return true;
}

void MAVSimThread::update()
{
	int maxFD, val, n=0;
	fd_set readFDs, writeFDs;
	struct timeval timeout;

	/*<------Select FDs------->*/
	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	val = select(maxFD + 1, &readFDs, &writeFDs, NULL, &timeout);

	if( val < 0 ) {
		if (errno != EINTR)
			cout << getName() << "::update() - select, " <<  strerror(errno) << endl;
	}
	else if(val == 0) {
	}
	/*<----One FD is Active--->*/
	else {
		/*<---RX Operations--->*/
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			if((n = readFifo(INBOARD_RX)) > 0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ) {
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_FP: {
								int num_waypts = cmd.size()/sizeof(Waypt_t);
								if(num_waypts > NUM_WAYPOINTS)
									num_waypts = NUM_WAYPOINTS;

								memcpy(&waypoints,dataPtr,num_waypts*sizeof(Waypt_t));
								current_waypoint = 0;

								// node is way to far away
								if(GPSRange(gps.pos,waypoints[current_waypoint].pos) > 20000) {
									gps.pos.lat = waypoints[current_waypoint].pos.lat;
									gps.pos.lon = waypoints[current_waypoint].pos.lon;
								}

								ACK(INBOARD_TX);

							} break;
						case CMD_CUSTODY: {
								Custody_t * custody  = (Custody_t*)dataPtr;

								if(custody->requesting) {
									if(parent == NULL) {
										parent = a_client;
									}

									if(parent != NULL) {
										custody->parent.s_addr = shmPtr->clients.getAddr(parent).s_addr;
									}
								}
								else {
									parent = NULL;
									custody->parent.s_addr = 0;
								}

								CMD_CSUM(fifo_msg.data) = 0;
								CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

								fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
								writeFifo(INBOARD_TX);
							} break;
						case CMD_TELEM: {

								if(parent != NULL)
									if(shmPtr->clients.getAddr(a_client).s_addr == shmPtr->clients.getAddr(parent).s_addr) {
										Telemetry_t * telemetry  = (Telemetry_t*)dataPtr;

										Waypt_t* waypoint = &(waypoints[0]);
										waypoint->number = 30;
										waypoint->next = 30;
										waypoint->orbitRad = 75;
										waypoint->pos.lat = telemetry->gps.pos.lat;
										waypoint->pos.lon = telemetry->gps.pos.lon;
										cout << "Set WP" << endl;

										current_waypoint = 30;
										cout << "TELEM (" << telemetry->gps.pos.lat << "," << telemetry->gps.pos.lon << ")" << endl;
									}

							} break;
						case CMD_COORDINATE: {
								cout << "CMD_COORDINATE" << endl;
								if(parent != NULL){
									if(shmPtr->clients.getAddr(a_client).s_addr == shmPtr->clients.getAddr(parent).s_addr) {
										Coordinate_t * coordinate  = (Coordinate_t*)dataPtr;

										if(coordinate->requesting) {
											if(partner == NULL) {
												cout << "Partner NULL" << endl;
												partner = shmPtr->clients.getClient(coordinate->partner);
												if(partner != NULL) {
													cout << "Partner a Client" << endl;
													reqPhase(partner, true);
													ACK(INBOARD_TX);
													cout << "Sent ACK to " << inet_ntoa(fifo_msg.dest) << endl;
												}
												else
													cout << "Partner NOT a Client: " << inet_ntoa(coordinate->partner) << endl;
											}
											else if(partner == shmPtr->clients.getClient(coordinate->partner)) {
												ACK(INBOARD_TX);
												cout << "Sent ACK to " << inet_ntoa(fifo_msg.dest) << endl;
											}
										}
										else {
											if(partner != NULL) {
												reqPhase(partner, false);
												partner = NULL;
											}
											ACK(INBOARD_TX);
											cout << "Sent ACK to " << inet_ntoa(fifo_msg.dest) << endl;
										}
									}
								}
							} break;
						case CMD_PHASE: {
								Phase_t * partner_phase  = (Phase_t*)dataPtr;
								speed = 15.0 + getVelocityRate(phase.angle,partner_phase->angle,0.03);

								if(speed < 10.0) speed = 10.0;
								else if(speed > 20.0) speed = 20.0;

								cout << "speed: " << speed << endl;
							} break;
					}
				}
			}
		}
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_telem_tx ) > (TELEMETRY_PERIOD * SEC2MICRO) ) {

				CMD_CMD((fifo_msg.data)) = CMD_GPS_TELEM;
				CMD_NUM((fifo_msg.data)) = sizeof(GPSTelem_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				Client * a_client;
				int i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_TELEM,i)) != NULL)
				{
					i++;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
				}
				last_telem_tx.stamp();
				moveMAV();

				CMD_CMD((fifo_msg.data)) = CMD_PHASE;
				CMD_NUM((fifo_msg.data)) = sizeof(Phase_t);
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum(fifo_msg.data);
				fifo_msg.size = CMD_SIZE(fifo_msg.data);

				i=0;
				while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_PHASE,i)) != NULL)
				{
					i++;
					fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
					writeFifo(INBOARD_TX);
				}
			}
		}
	}
}

void MAVSimThread::moveMAV()
{
	float turn_rate = 0;
	Velocity_t vdes;
	float psides;
	float psidotdes;
	float heading = atan2(velocity.y, velocity.x);

	vectorField(gps.pos, heading, waypoints[current_waypoint].pos, waypoints[current_waypoint].orbitRad, speed, &vdes, &psides, &psidotdes);

	turn_rate = getTurnRate((float)gps.track*M_PI/180, psides, psidotdes, 0.5);

	velocity.x = cos(heading+turn_rate)*speed;
	velocity.y = sin(heading+turn_rate)*speed;

	gps.pos.lat += M_TO_LAT(velocity.x);
	gps.pos.lon += M_TO_LON(velocity.y, gps.pos.lat);
	gps.track = atan2(velocity.y, velocity.x)*180/M_PI;
	gps.sog = speed;

	GPSLatLon_t delx;

	float r = GPSRange( gps.pos, waypoints[current_waypoint].pos);

	if(r < (waypoints[current_waypoint].orbitRad) + speed)
		current_waypoint = waypoints[current_waypoint].next;

	phase.angle = atan2(delx.lat,delx.lon)*180/M_PI;
}

void MAVSimThread::reqPhase(Client * a_client, bool toggle) {
	cout << "Requesting Phase" << endl;
	Client * me = shmPtr->clients.getClient(0);
	//Client * a_client = shm_struct->clients.getClient(ind);
	CMD_CMD(fifo_msg.data) = CMD_STREAM_REQ;
	CMD_NUM(fifo_msg.data) = sizeof(StreamReq_t);
	StreamReq_t * request = (StreamReq_t*)CMD_DATAPTR(fifo_msg.data);

	request->id = CMD_PHASE;
	request->client.s_addr = shmPtr->clients.getAddr(me).s_addr;
	request->port = 1000;
	request->send_data = toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
}

MAVSimThread::~MAVSimThread()
{
}

